/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2016
*  TU Dortmund - Institute of Control Theory and Systems Engineering.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the institute nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann, Otniel Rinaldo
*********************************************************************/

#ifndef COSTMAP_CONVERTER_INTERFACE_H_
#define COSTMAP_CONVERTER_INTERFACE_H_

//#include <costmap_2d/costmap_2d_ros.h>
#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <boost/thread.hpp>
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <geometry_msgs/Polygon.h>
#include <costmap_converter/ObstacleArrayMsg.h>

namespace costmap_converter
{

//! Typedef for a shared dynamic obstacle container
typedef boost::shared_ptr<ObstacleArrayMsg> ObstacleArrayPtr;
//! Typedef for a shared dynamic obstacle container (read-only access)
typedef boost::shared_ptr<const ObstacleArrayMsg> ObstacleArrayConstPtr;
//! Typedef for a shared polygon container
typedef boost::shared_ptr<std::vector<geometry_msgs::Polygon> > PolygonContainerPtr;
//! Typedef for a shared polygon container (read-only access)
typedef boost::shared_ptr<const std::vector<geometry_msgs::Polygon> > PolygonContainerConstPtr;

/** 这个抽象类定义了将代价地图转换为多边形类型的插件接口
* @class BaseCostmapToPolygons
* @brief This abstract class defines the interface for plugins that convert the costmap into polygon types
*
* Plugins must accept a costmap_2d::Costmap2D datatype as information source.
* The interface provides two different use cases:
* 1. Manual call to conversion routines: setCostmap2D(), compute() and getPolygons()
*    (in subsequent calls setCostmap2D() can be substituted by updateCostmap2D())
* 2. Repeatedly process costmap with a specific rate (startWorker() and stopWorker()):
*    Make sure that the costmap is valid while the worker is active (you can specify your own spinner or activate a threaded spinner).
*    Costmaps can be obtained by invoking getPolygons().
*/
class BaseCostmapToPolygons
{
public:
    /**
     * @brief Initialize the plugin
     * @param nh Nodehandle that defines the namespace for parameters
     */
    virtual void initialize(ros::NodeHandle nh) = 0;

    /**
     * @brief Destructor
     */
    virtual ~BaseCostmapToPolygons() { stopWorker(); }

    /**
     * @brief Pass a pointer to the costap to the plugin.
     * @warning The plugin should handle the costmap's mutex locking.
     * @sa updateCostmap2D
     * @param costmap Pointer to the costmap2d source
     */
    virtual void setCostmap2D(costmap_2d::Costmap2D *costmap) = 0;

    /**
     * @brief Get updated data from the previously set Costmap2D
     * @warning The plugin should handle the costmap's mutex locking.
     * @sa setCostmap2D
     */
    virtual void updateCostmap2D() = 0;

    /**
    * @brief This method performs the actual work (conversion of the costmap to polygons)
    */
    virtual void compute() = 0;

    /**
     * @brief Get a shared instance of the current polygon container
     *
     * If this method is not implemented by any subclass (plugin) the returned shared
     * pointer is empty.
     * @remarks If compute() or startWorker() has not been called before, this method returns an empty instance!
     * @warning The underlying plugin must ensure that this method is thread safe.
     * @return Shared instance of the current polygon container
     */
    virtual PolygonContainerConstPtr getPolygons() { return PolygonContainerConstPtr(); }

    /**
     * @brief Get a shared instance of the current obstacle container
     * If this method is not overwritten by the underlying plugin, the obstacle container just imports getPolygons().
     * @remarks If compute() or startWorker() has not been called before, this method returns an empty instance!
     * @warning The underlying plugin must ensure that this method is thread safe.
     * @return Shared instance of the current obstacle container
     * @sa getPolygons
     */
    virtual ObstacleArrayConstPtr getObstacles()
    {
        ObstacleArrayPtr obstacles = boost::make_shared<ObstacleArrayMsg>();
        PolygonContainerConstPtr polygons = getPolygons();
        if (polygons)
        {
            for (const geometry_msgs::Polygon &polygon : *polygons)
            {
                obstacles->obstacles.emplace_back();
                obstacles->obstacles.back().polygon = polygon;
            }
        }
        return obstacles;
    }

    /** 一些插件可能需要里程计信息来补偿机器人的自我运动
     * @brief Set name of robot's odometry topic
     *
     * Some plugins might require odometry information to compensate the robot's ego motion
     * @param odom_topic topic name
     */
    virtual void setOdomTopic(const std::string &odom_topic) {}

    /**
     * @brief Determines whether an additional plugin for subsequent costmap conversion is specified
     *
     * @return false, since all plugins for static costmap conversion are independent
     */
    virtual bool stackedCostmapConversion() { return false; }

    /**
     * @brief Instantiate a worker that repeatedly coverts the most recent costmap to polygons.
     * The worker is implemented as a timer event that is invoked at a specific \c rate.
     * The passed \c costmap pointer must be valid at the complete time and must be lockable.
     * By specifying the argument \c spin_thread the timer event is invoked in a separate
     * thread and callback queue or otherwise it is called from the global callback queue (of the
     * node in which the plugin is used).
     * @param rate The rate that specifies how often the costmap should be updated
     * @param costmap Pointer to the underlying costmap (must be valid and lockable as long as the worker is active
     * @param spin_thread if \c true,the timer is invoked in a separate thread, otherwise in the default callback queue)
    */
    void startWorker(ros::Rate rate, costmap_2d::Costmap2D *costmap, bool spin_thread = false)
    {
        setCostmap2D(costmap);
        if (spin_thread_)
        {
            {
                boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
                need_to_terminate_ = true;
            }
            spin_thread_->join();
            delete spin_thread_;
        }
        if (spin_thread)
        {
            ROS_DEBUG_NAMED("costmap_converter", "Spinning up a thread for the CostmapToPolygons plugin");
            need_to_terminate_ = false;
            spin_thread_ = new boost::thread(boost::bind(&BaseCostmapToPolygons::spinThread, this));
            nh_.setCallbackQueue(&callback_queue_);
        }
        else
        {
            spin_thread_ = NULL;
            nh_.setCallbackQueue(ros::getGlobalCallbackQueue());
        }
        worker_timer_ = nh_.createTimer(rate, &BaseCostmapToPolygons::workerCallback, this);
    }

    /**
     * @brief Stop the worker that repeatedly converts the costmap to polygons
     */
    void stopWorker()
    {
        worker_timer_.stop();
        if (spin_thread_)
        {
            {
                boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
                need_to_terminate_ = true;
            }
            spin_thread_->join();
            delete spin_thread_;
        }
    }

protected:

    /**
     * @brief Protected constructor that should be called by subclasses
     */
    BaseCostmapToPolygons() : nh_("~costmap_to_polygons"), spin_thread_(NULL), need_to_terminate_(false) {}

    /**
     * @brief Blocking method that checks for new timer events (active if startWorker() is called with spin_thread enabled)
     */
    void spinThread()
    {
        while (nh_.ok())
        {
            {
                boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
                if (need_to_terminate_) break;
            }
            callback_queue_.callAvailable(ros::WallDuration(0.1f));
        }
    }

    /**
     * @brief The callback of the worker that performs the actual work (updating the costmap and converting it to polygons)
     */
    void workerCallback(const ros::TimerEvent &)
    {
        updateCostmap2D();
        compute();
    }

private:
    ros::Timer worker_timer_;
    ros::NodeHandle nh_;
    boost::thread *spin_thread_;
    ros::CallbackQueue callback_queue_;
    boost::mutex terminate_mutex_;
    bool need_to_terminate_;
};

/**
* @class BaseCostmapToDynamicPolygons
* @brief This class extends the BaseCostmapToPolygongs class for dynamic costmap conversion plugins
*        in order to incorporate a subsequent costmap converter plugin for static obstacles
* This class should not be instantiated.
*/
class BaseCostmapToDynamicObstacles : public BaseCostmapToPolygons
{
public:

    /**
     * @brief Load underlying static costmap conversion plugin via plugin-loader
     * @param plugin_name Exact class name of the plugin to be loaded, e.g.
     *                    costmap_converter::CostmapToPolygonsDBSMCCH
     * @param nh_parent   NodeHandle which is extended by the namespace of the static conversion plugin
     */
    void loadStaticCostmapConverterPlugin(const std::string &plugin_name, ros::NodeHandle nh_parent)
    {
        try
        {
            static_costmap_converter_ = static_converter_loader_.createInstance(plugin_name);
            if (boost::dynamic_pointer_cast<BaseCostmapToDynamicObstacles>(static_costmap_converter_))
            {
                throw pluginlib::PluginlibException(
                        "The specified plugin for static costmap conversion is a dynamic plugin. Specify a static plugin.");
            }
            std::string raw_plugin_name = static_converter_loader_.getName(plugin_name);
            static_costmap_converter_->initialize(ros::NodeHandle(nh_parent, raw_plugin_name));
            setStaticCostmapConverterPlugin(static_costmap_converter_);
            ROS_INFO_STREAM("CostmapToDynamicObstacles: underlying costmap conversion plugin for static obstacles "
                                    << plugin_name << " loaded.");
        }
        catch (const pluginlib::PluginlibException &ex)
        {
            ROS_WARN("CostmapToDynamicObstacles: The specified costmap converter plugin cannot be loaded. "
                     "Continuing without subsequent conversion of static obstacles. Error message: %s",
                     ex.what());
            static_costmap_converter_.reset();
        }
    }

    /**
     * @brief Set the underlying plugin for subsequent costmap conversion of the static background of the costmap
     * @param static_costmap_converter shared pointer to the static costmap conversion plugin
     */
    void setStaticCostmapConverterPlugin(boost::shared_ptr<BaseCostmapToPolygons> static_costmap_converter)
    {
        static_costmap_converter_ = static_costmap_converter;
    }

    /**
     * @brief Set the costmap for the underlying plugin
     * @param static_costmap Costmap2D, which contains the static part of the original costmap
     */
    void setStaticCostmap(boost::shared_ptr<costmap_2d::Costmap2D> static_costmap)
    {
        static_costmap_converter_->setCostmap2D(static_costmap.get());
    }

    /**
     * @brief Apply the underlying plugin for static costmap conversion
     */
    void convertStaticObstacles() { static_costmap_converter_->compute(); }

    /**
     * @brief Get the converted polygons from the underlying plugin
     * @return Shared instance of the underlying plugins polygon container
     */
    PolygonContainerConstPtr getStaticPolygons()
    {
        return static_costmap_converter_->getPolygons();
    }

    /**
     * @brief Determines whether an additional plugin for subsequent costmap conversion is specified
     *
     * @return true, if a plugin for subsequent costmap conversion is specified
     */
    bool stackedCostmapConversion()
    {
        if (static_costmap_converter_) return true;
        else return false;
    }

protected:
    /**
     * @brief Protected constructor that should be called by subclasses
     */
    BaseCostmapToDynamicObstacles() :
            static_converter_loader_("costmap_converter", "costmap_converter::BaseCostmapToPolygons"),
            static_costmap_converter_() {}

private:
    pluginlib::ClassLoader<BaseCostmapToPolygons> static_converter_loader_;
    boost::shared_ptr<BaseCostmapToPolygons> static_costmap_converter_;
};

}

#endif // end COSTMAP_CONVERTER_INTERFACE_H_
